#include "LSM303M.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "rpi.h"

#define LSM303M_ADDRESS            (0x3C >> 1)

#define LSM303M_CRA_REG_M         0x00
#define LSM303M_CRB_REG_M         0x01
#define LSM303M_MR_REG_M          0x02

#define LSM303M_OUT_X_H_M         0x03
#define LSM303M_OUT_X_L_M         0x04
#define LSM303M_OUT_Y_H_M         0x07
#define LSM303M_OUT_Y_L_M         0x08
#define LSM303M_OUT_Z_H_M         0x05
#define LSM303M_OUT_Z_L_M         0x06

#define LSM303M_SR_REG_M          0x09
#define LSM303M_IRA_REG_M         0x0A
#define LSM303M_IRB_REG_M         0x0B
#define LSM303M_IRC_REG_M         0x0C

#define LSM303M_WHO_AM_I_M        0x0F







LSM303M::LSM303M(int file) : I2CDevice(file, LSM303M_ADDRESS)
{
	// Enable Magnetometer
	// 0x00 = 0b00000000
	// Continuous conversion mode
  // 0b00011000
  writeByte(LSM303M_CRA_REG_M, 0x18);
  writeByte(LSM303M_MR_REG_M, 0x00);

}





// Reads the 3 magnetometer channels and stores them in vector m
void LSM303M::read()
{


  __u8  buf[6];

  readBlock(LSM303M_OUT_X_H_M, buf, 6);

  for (int i = 0; i < 6; i++)
  {
    buf[i] = ~buf[i];
    buf[i] += 0x01;
  }
  /*
  printf("mag: ");
  for (int i = 0; i < 6; i++)
    printf("%02x ", buf[i]);
  printf("\n");
  */

  __u8 xhm = buf[0];
  __u8 xlm = buf[1];
	
	__u8 yhm, ylm, zhm, zlm;
	
  zhm = buf[2];
  zlm = buf[3];
  yhm = buf[4];
  ylm = buf[5];

  __s16 x = xhm << 8 | xlm;
  __s16 y = yhm << 8 | ylm;
  __s16 z = zhm << 8 | zlm;

	m.x() = x;
	m.y() = y;
	m.z() = z;
}


